#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <resolv.h>
#include <errno.h>

#define BUF_SZ 1024

typedef struct {
  int cmd;

  union {
#define CMD_KILL_REQUEST 0    
    struct {
      int dummy;
    } KillRequest;

#define CMD_MOVE_REQUEST 1    
    struct {
      int joint;
      unsigned char pos;
    } MoveRequest;
  } u;
} RCI_CMD;

int main(int argc, char **argv)
{
  int sockfd;
  struct sockaddr_in dest;
  RCI_CMD rci_cmd;
  int joint, pos;

  // Make sure we have the right number of parameters
  if (argc < 5)
  {
    fprintf(stderr, "usage: %s <IP-addr> <port> <servo> <pos>\n", argv[0]);
    exit(0);
  }

  // Create socket for streaming
  if ( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
    perror("Socket");
    exit(errno);
  }

  // Initialize server address/port struct
  bzero(&dest, sizeof(dest));
  dest.sin_family = AF_INET;
  if ( (dest.sin_addr.s_addr = inet_addr(argv[1])) == 0)
  {
    perror(argv[1]);
    exit(errno);
  }
  dest.sin_port = htons(atoi(argv[2]));

  // Connect to server
  if (connect(sockfd, (struct sockaddr *)&dest, sizeof(dest)) != 0)
  {
    perror("Connect");
    exit(errno);
  }

  // Fill out rci cmd
  joint = atoi(argv[3]);
  pos = atoi(argv[4]);

  if (joint == 0xFF)
  {
    rci_cmd.cmd = CMD_KILL_REQUEST;
  }
  else
  {
    rci_cmd.cmd = CMD_MOVE_REQUEST;
    rci_cmd.u.MoveRequest.joint = atoi(argv[3]);
    rci_cmd.u.MoveRequest.pos = atoi(argv[4]);
  }
  
  send(sockfd, &rci_cmd, sizeof(RCI_CMD), 0);

  // Clean up
  close(sockfd);
  return 0;
}

